#include <wiringPi.h>
#include <stdio.h>

int main()
{
	const int trigger_pin=4;
	const int echo_pin=5;
	const int button_pin=21;

	wiringPiSetup();

	pinMode(echo_pin, INPUT);
	pinMode(trigger_pin, OUTPUT);
	digitalWrite(trigger_pin, LOW);
	pinMode(button_pin, INPUT);

	unsigned int echo_start=0, echo_end=0;
	int input_prev=HIGH;
	int ultra=0;

	while(1){
		int input_cur=digitalRead(button_pin);

		if(input_prev==HIGH && input_cur==LOW)
			ultra=1;

		if(input_prev!=input_cur)
			input_prev=input_cur;

		if(ultra==1){
			int n_repeats=5;
			double sum=0;

			for(int i=0;i<n_repeats;++i){
				digitalWrite(trigger_pin, HIGH);
				delayMicroseconds(10);
				digitalWrite(trigger_pin, LOW);

				while(digitalRead(echo_pin)==LOW)
					echo_start=micros();

				while(digitalRead(echo_pin)==HIGH)
					echo_end=micros();

				double distance=(331.5+0.61*25)*((echo_end-echo_start)/2.0/1000000)*100;

				sum+=distance;
			}

			printf("Distance: %.2f cm\n", sum/n_repeats);
			ultra=0;
		}

		delay(20);
	}

	return 0;
}